#ifndef VL_FP_HPP
#define VL_FP_HPP

#error "This is not working!"

#include <cassert>

namespace vl
{

typedef unsigned int fp_t;


inline fp_t fp_add_intern(fp_t a, fp_t b, fp_t ae, int sh)
{
	fp_t bm = (b & MantissaMask) >> sh; // TODO: Make it signed shift
	
	fp_t am = (((a & MantissaMask) >> 1) + b);
	
	
}

fp_t const ExponentMask = ((1 << 8) - 1);
fp_t const MantissaMask = ((1 << 23) - 1) << 8;
fp_t const SignMask = 0x80000000;

inline fp_t fp_e(fp_t a)
{
	return a & ExponentMask;
}

inline fp_t fp_sub_intern(fp_t a, fp_t b, fp_t ae, int sh)
{
	// Here we can assume a few things:
	// fp_e(a) >= fp_e(b)
	
	// Align the mantissa of b
	b = (b & MantissaMask) >> sh;
	
	// Calculate new mantissa
	fp_t am = ((a & MantissaMask) - b) & (MantissaMask | SignMask);
	
	if(am >= 0x80000000) // Underflow
	{
		// Sign change
		am = ~am + (1 << 23);
		a ^= SignMask;
	}
	else if(!am)
		return 0;

	// Normalize
	
	while(am < 0x80000000)
	{
		if(ae == 0)
		{
			return 0; // NOTE: This prevents subnormal numbers, but hurts precision
		}
		
		am <<= 1;
		--ae;
	}
	
	return (a & SignMask) | am | ae;
}

inline fp_t fp_add_intern(fp_t a, fp_t b, fp_t ae, int sh)
{
	// Here we can assume a few things:
	// fp_e(a) >= fp_e(b)
	
	// Align the mantissa of b
	b = (b & MantissaMask) >> sh;
	
	// Calculate new mantissa
	fp_t am = (a & MantissaMask) + b;
	
	fp_t of = (am >> 31) // Overflow
		
	return (a & SignMask) | ((am >> of) & MantissaMask) | (ae + of);
}

fp_t fp_add(fp_t a, fp_t b)
{
	fp_t ae = fp_e(a);
	int sh = fp_e(b) - ae;
	if(sh < 0)
	{
		if((b ^ a) < SignMask) // Opposite signs, do a -= -b; instead
			return fp_sub_intern(b, a, ae, -sh);
		
		return fp_add_intern(b, a, ae, -sh);
	}
	else
	{
		if((a ^ b) < SignMask) // Opposite signs, do a -= -b; instead
			return fp_sub_intern(a, b, ae, sh);
		
		return fp_add_intern(a, b, ae, sh);
	}
}

fp_t fp_sub(fp_t a, fp_t b)
{
	fp_t ae = fp_e(a);
	int sh = fp_e(b) - ae;
	if(sh < 0)
	{
		if((b ^ a) >= SignMask) // Same signs, do a += -b; instead
			return fp_add_intern(b, a, ae, -sh);
			
		return fp_sub_intern(b, a, ae, -sh);
	}
	else
	{	
		if((a ^ b) >= SignMask) // Same signs, do a += -b; instead
			return fp_add_intern(a, b, ae, sh);
			
		return fp_sub_intern(a, b, ae, sh);
	}
}

struct fp
{
	fp()
	: v(0)
	{
	}
		
	fixed operator-() const
	{ return fixed(-v, literal_tag()); }
	
	fixed& operator+=(fixed b)
	{ v += b.v;	return *this; }
	
	fixed operator+(fixed b) const
	{ return fixed(*this) += b; }
	
	fixed& operator-=(fixed b)
	{ v -= b.v;	return *this; }
	
	fixed operator-(fixed b) const
	{ return fixed(*this) -= b; }
	
	fixed& operator*=(fixed b)
	{
		v = static_cast<int>((static_cast<long long>(v) * b.v) >> 16);
		return *this;
	}
	
	fixed& operator/=(fixed b)
	{
		v = static_cast<int>((static_cast<long long>(v) << 16) / b.v);
		return *this;
	}
	
	fixed operator*(fixed b) const
	{ return fixed(*this) *= b; }
	
	fixed operator/(fixed b) const
	{ return fixed(*this) /= b; }
	
	fixed half() const
	{
		return from_literal(v / 2);
	}
	
	int to_int() const
	{
		return (v >> 16);
	}
	
	double to_double() const
	{
		return v / 65536.0;
	}
	
	int to_rep() const
	{
		return v;
	}
	
	angle to_angle() const
	{
		return angle((*this * vl::fixed(fixed_2pi, literal_tag())).to_rep());
	}
	
	static fixed from_literal(int v)
	{
		return fixed(v, literal_tag());
	}
	
	static int cos_table[4096];
	static int sin_table[4096];
	static int angle_table[4097];
	
	static fixed cos(angle a)
	{
		return from_literal(cos_table[a.adjust<12>() & 4095]);
	}
	
	static fixed sin(angle a)
	{
		return from_literal(sin_table[a.adjust<12>() & 4095]);
	}
	
	static basic_vec<fixed> vec_of(angle a)
	{
		return basic_vec<fixed>(cos(a), sin(a));
	}
	
	static angle angle_of(fixed x, fixed y);
	
	static void generate_tables();
	static void print_tables();
	
	bool operator<(fixed b) const { return v < b.v; }
	bool operator>(fixed b) const { return v > b.v; }
	bool operator<=(fixed b) const { return v <= b.v; }
	bool operator>=(fixed b) const { return v >= b.v; }
	bool operator==(fixed b) const { return v == b.v; }
	bool operator!=(fixed b) const { return v != b.v; }
	
	operator bool() const
	{
		return v != 0;
	}
	
	int v;
};

angle angle_of(basic_vec<fixed> v);

}

#endif // VL_FIXED_HPP
